
#include "../matrix/gsl_ext_matrix.h"
#include "gsl_ext_linalg.h"

#include <gsl/gsl_machine.h>
#include <gsl/gsl_math.h>
#include <math.h>

int gsl_ext_linalg_modified_cholesky_decomp(const gsl_matrix *H, gsl_matrix *L, gsl_vector *e)
{
  const int n = H->size1;
  
  double gamma = gsl_ext_matrix_get_max_abs_diag_element(H);
  double xi = gsl_ext_matrix_get_max_abs_offdiag_element(H);
  double beta2;
  double delta = GSL_DBL_EPSILON;
  double tmpsum;
  int i;
  int j;
  int k;
  double theta;
  double phi;
  
  if(gamma > xi / sqrt(n*n - 1.0) && gamma > GSL_DBL_EPSILON)
    beta2 = gamma;
  else if(xi / sqrt(n*n - 1.0) > gamma && xi / sqrt(n*n - 1.0) > GSL_DBL_EPSILON)
    beta2 = xi / sqrt(n*n - 1.0);
  else
    beta2 = GSL_DBL_EPSILON;
  
  for(j = 0; j < n; j++)
  {
    tmpsum = 0.0;
    for(k = 0; k < j; k++)
      tmpsum += gsl_matrix_get(L, j, k) * gsl_matrix_get(L, j, k) * gsl_matrix_get(L, k, k);
    phi = gsl_matrix_get(H, j, j) - tmpsum;
    
    theta = 0.0;
    for(i = j + 1; i < n; i++)
    {
      tmpsum = 0.0;
      for(k = 0; k < j; k++)
        tmpsum += gsl_matrix_get(L, j, k) * gsl_matrix_get(L, i, k) * gsl_matrix_get(L, k, k);
      theta = GSL_MAX_DBL(fabs(gsl_matrix_get(H, i, j) - tmpsum), theta);
      //theta = GSL_MAX_DBL(fabs(gsl_matrix_get(L, i, j) * gsl_matrix_get(L, j, j)), theta);
    }
    
    gsl_matrix_set(L, j, j, GSL_MAX_DBL(GSL_MAX_DBL(fabs(phi), delta), theta * theta / beta2));
    gsl_vector_set(e, j, gsl_matrix_get(L, j, j) - phi);
    
    for(i = j + 1; i < n; i++)
    {
      tmpsum = 0.0;
      for(k = 0; k < j; k++)
        tmpsum += gsl_matrix_get(L, j, k) * gsl_matrix_get(L, i, k) * gsl_matrix_get(L, k, k);
    
      gsl_matrix_set(L, i, j, (gsl_matrix_get(H, i, j) - tmpsum) / gsl_matrix_get(L, j, j));
    }
  }
  
  return GSL_SUCCESS;
}
